/*
===========================================================================
This source file is part of DyLab (Dynamic Laboratory)
For the latest info, see http://dylab-modules.googlecode.com

Copyright (c) 2006-2008 Lukas Krejci
(krejci.lukas@volny.cz)

This file is part of DyLab.

    DyLab is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    DyLab is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with DyLab.  If not, see <http://www.gnu.org/licenses/>.
===========================================================================
*/

//===========================================================================================
#include "Plane_CollisionHandler.h"
#include <dylab/simobject/masspoints/MassPointsFeatureBase.h>
#include <dylab/simulation/Simulation.h>

//===========================================================================================
bool dylab::Plane_CollisionHandler::PlaneCollider::collide(const Vector3 & _currPos, 
	const Vector3 & _prevPos, Vector3 * _correctPos, real_t _friction)
{
	// test, if the point is "under" the plane
	real_t currPosSigPlaneDis = mPlane.computeSignedDistance(_currPos);
    if (currPosSigPlaneDis < 0)
    {
		// compute correct position only if it is needed
        if (_correctPos != NULL)
		{	
			// compute plane projection of the position
			Vector3 currPosPlaneProj = _currPos - mPlane.getNormal() * currPosSigPlaneDis;

			// if the previous point is inside the plane
			if (mPlane.testPointInside(_prevPos))
			{				
				// correct the position only by putting it on the plane
				*_correctPos = currPosPlaneProj + (_prevPos - currPosPlaneProj) * _friction;
			}
			else
			{					
				// the point was moving, so bounce it by deflecting the part of its trajectory that goes under the plane
				Vector3 correctedOnPlaneProj = currPosPlaneProj + (_prevPos - currPosPlaneProj) * _friction;
				*_correctPos = correctedOnPlaneProj - mPlane.getNormal() * (currPosSigPlaneDis * (1 - _friction));
			}
		}

        return true;
    }	

    return false;
}

//===========================================================================================
const dylab::String dylab::Plane_CollisionHandler::COMPONENT_NAME = "plane";

//===========================================================================================
dylab::Plane_CollisionHandler::Plane_CollisionHandler() 
: ComCollisionHandler(COMPONENT_NAME), mParentSimulation(NULL), mPlaneColliders(true), mFriction(DYLAB_R(0.5))
{
}
//-----------------------------------------------------------------------------
dylab::Plane_CollisionHandler::~Plane_CollisionHandler() 
{
    mPlaneColliders.removeAll();
}
//-----------------------------------------------------------------------------
void dylab::Plane_CollisionHandler::initialize(Simulation * _parentSimulation)
{
    // check & store parent simulation
    DYLAB_ASSERT(_parentSimulation != NULL);
    mParentSimulation = _parentSimulation;
}
//-----------------------------------------------------------------------------
void dylab::Plane_CollisionHandler::loadFromPropertyTree(const PropertyTree & _pt)
{
    // the friction
    setFriction(_pt.getNodeValueT("friction", mFriction));

    // the planes itself
    for (PropertyTree::NodeList::ConstIterator plane(*_pt.getNodeList("plane")); plane.isValid(); plane++)
    {
        // retrieve the plane normal
        Vector3 planeNormal = plane->getChildNodeValueT<Vector3>("normal");
        planeNormal.normalise();

        // in order to construct the plane, the displacement or point is needed yet
        PropertyTree::Node * displacement = plane->findChildNode("displacement");
        PropertyTree::Node * point = plane->findChildNode("point");
        if (displacement != NULL)         
            addPlane(plane->getValue(), Plane(planeNormal, displacement->getValue().parse<real_t>()));        
        else if (point != NULL)                                   
            addPlane(plane->getValue(), Plane(planeNormal, point->getValue().parse<Vector3>()));        
    }
}
//-----------------------------------------------------------------------------
dylab::Simulation * dylab::Plane_CollisionHandler::getParentSimulation()
{
    return mParentSimulation;
}
//-----------------------------------------------------------------------------
const dylab::Simulation * dylab::Plane_CollisionHandler::getParentSimulation() const
{
    return mParentSimulation;
}
//-----------------------------------------------------------------------------
void dylab::Plane_CollisionHandler::handleCollisions()
{
    for (Simulation::SimObjects::Iterator simObj(mParentSimulation->simObjectIterator()); simObj.isValid(); simObj++)
    {
        MassPointsFeatureBase::ScopedExLock iMassPoints(simObj->getModelStructure());
		ComModelIntegrator::ScopedExLock	  modelIntegrator(simObj->getModelIntegrator());

        // walk through all mass points
        for (MassPointContainer::Iterator mp(iMassPoints->massPointIterator()); mp.isValid(); mp++)
        {
            for (PlaneColliderMap::Iterator plane(mPlaneColliders); plane.isValid(); plane++)
            {
                Vector3 newPos;
                if (plane->collide(mp->currState().position, mp->historyState(1).position, &newPos, mFriction))
                {
                    // correct the mass point's position
					modelIntegrator->correctPosition(newPos, mp());

					//TODO if there was a deflection then do not exectute the above line, but instead:
					// the incoming point trajectory
					//t1 = modelIntegrator->getLastTimeStep() * firstPartRatio;
					//modelIntegrator->corectPosition(planeIntersection, mp(), t1);
					//mp->storeState();
					// the deflected point trajectory
					//t2 = modelIntegrator->getLastTimeStep() * secondPartRatio;
					//modelIntegrator->correctPosition(newPos, mp(), t2);
					//OR with the help of new integrator method:
					//modelIntegrator->correctPositionWithTrajectorySubdivision(planeIntersection, newPos, mp());
                }
            }
        }
    }
}
//-----------------------------------------------------------------------------